//
// Created by StephenZzz on 2025/2/25.
//

#include "wf_manager.h"

#include <future>
#include <iostream>
#include <memory>
#include <ostream>
#include <unistd.h>
#include <thread>

#include <daf/daf_model.h>
#include "gazella/logger.h"
#include "daf/DeviceAgentModel.pb.h"

#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action_impl.h>
#include <mavsdk/plugins/manual_control/manual_control_impl.h>
#include <mavsdk/plugins/mission/mission.h>
#include <mavsdk/plugins/telemetry/telemetry_impl.h>

using namespace Gazella;
using namespace Daf;
using namespace mavsdk;


namespace WfAgent
{
    Mavsdk _mavsdk{Mavsdk::Configuration{Mavsdk::ComponentType::GroundStation}};
    std::shared_ptr<System> g_system;

    WfManager* WfManager::getInstance()
    {
        static WfManager instance;
        return &instance;
    }

    WfManager::WfManager()
    = default;

    WfManager::~WfManager()
    {
        shutdown();
    }

    // shutdown() 方法：安全停止后台线程
    void WfManager::shutdown()
    {
        m_stopTelemetry = true;
        if (m_telemetryThread.joinable())
        {
            m_telemetryThread.join();
        }
    }


    bool WfManager::init()
    {
        while (1)
        {
            std::string connection_url = "serial:///dev/ttyS9:115200";
            ConnectionResult connection_result = _mavsdk.add_any_connection(connection_url);
            if (connection_result != ConnectionResult::Success)
            {
                mp_log_info(DEFAULT_LOGGER, "Connection failed: %d", connection_result);
            }
            else
            {
                printf("microUAV Connection Success");
                set_device_status(DeviceStatusTbl::DEVICE_STATE_NORMAL);
                break;
            }

            mp_log_info(DEFAULT_LOGGER, "connection result: %d", static_cast<int>(connection_result));
            sleep(2);
        }
        while (_mavsdk.systems().empty())
        {
            mp_log_info(DEFAULT_LOGGER, "init g_system failed!");
            std::this_thread::sleep_for(std::chrono::seconds(1));
        }
        g_system = _mavsdk.systems().at(0);
        mp_log_info(DEFAULT_LOGGER, "init g_system success!");


        m_wf_action = std::make_shared<ActionImpl>(g_system);
        m_wf_manual_control = std::make_shared<ManualControlImpl>(g_system);
        m_wf_mission = std::make_shared<Mission>(g_system);

        // 初始化 Telemetry 插件对象，并保存为成员变量
        m_telemetry = std::make_shared<TelemetryImpl>(g_system);

        // 启动后台线程，执行 telemetry_callback_loop()（采用受控线程，非 detached）
        m_stopTelemetry = false;
        m_telemetryThread = std::thread(&WfManager::telemetry_callback_loop, this);

        m_default_manual_thread = std::thread(&WfManager::default_manual_control_loop, this);

        mp_log_info(DEFAULT_LOGGER, "microUAV WfManager init ending");
        return true;
    }

    void WfManager::set_device_status(DeviceStatusTbl status)
    {
        DeviceAgentModel::DeviceAgentProperty statusP;
        DafModel::get_instance()->get_property("status", statusP);
        //"DEVICE_STATE_UNKNOWN = 0; DEVICE_STATE_NORMAL = 1;DEVICE_STATE_OFFLINE = 2;DEVICE_STATE_FAULT = 3;DEVICE_STATE_BUSY = 4;DEVICE_STATE_IDLE = 5;",
        statusP.mutable_property_int()->set_value(static_cast<google::protobuf::int32>(status));
        DafModel::get_instance()->set_property("status", statusP);
    }

    // 需要默认发送一个摇杆指令
    void WfManager::default_manual_control_loop()
    {
        while (true)
        {
            // auto result = m_wf_manual_control->set_manual_control_input(0.0f, 0.0f, 0.0f, 0.5f);
            auto ret = m_wf_manual_control->set_manual_control_input(joy_cache.wf_pitch, joy_cache.wf_roll,
                                                                     joy_cache.wf_throttle, joy_cache.wf_yaw);
            std::this_thread::sleep_for(std::chrono::milliseconds(250));
            mp_log_info(DEFAULT_LOGGER, "default_manual_control_loop ret:%d", ret);
        }
    }

    void WfManager::telemetry_callback_loop()
    {
        std::cout << "WfManager::get_device_info()" << std::endl;


        // 定义一个 lambda 用于注册所有回调
        auto register_telemetry_callbacks = [this]()
        {
            // 解锁状态
            m_telemetry->subscribe_armed(std::bind(&WfManager::armd_callback, std::placeholders::_1));

            // 电量
            m_telemetry->subscribe_battery(std::bind(&WfManager::battery_status_callback, std::placeholders::_1));

            // 位置
            m_telemetry->subscribe_position(std::bind(&WfManager::position_callback, std::placeholders::_1));

            // 速度
            m_telemetry->subscribe_position_velocity_ned(
                std::bind(&WfManager::position_velocity_ned_callback, std::placeholders::_1));

            // 姿态角速度信息
            m_telemetry->subscribe_attitude_angular_velocity_body(
                std::bind(&WfManager::attitude_angular_velocity_callback, std::placeholders::_1));

            // 姿态角信息
            m_telemetry->subscribe_attitude_euler(std::bind(&WfManager::attitude_euler_callback,
                                                            std::placeholders::_1));

            // 返航点信息
            m_telemetry->subscribe_home(std::bind(&WfManager::home_position_callback, std::placeholders::_1));

            // 搜星数
            m_telemetry->subscribe_gps_info([](auto && PH1) { WfManager::gps_info_callback(std::forward<decltype(PH1)>(PH1)); });

            // 飞行模式
            m_telemetry->
                subscribe_flight_mode(std::bind(&WfManager::flight_mode_callback, std::placeholders::_1));
        };
        register_telemetry_callbacks();

        while (!m_stopTelemetry)
        {
            // 检查系统对象是否仍然有效及连接是否正常
            if (!g_system || !g_system->is_connected())
            {
                mp_log_info(DEFAULT_LOGGER, "UAV lost Connection, trying to reconnected");
                set_device_status(DeviceStatusTbl::DEVICE_STATE_OFFLINE);
                // 进入等待重连循环
                while (!m_stopTelemetry && (!g_system || !g_system->is_connected()))
                {

                    std::this_thread::sleep_for(std::chrono::seconds(2));
                    // 尝试重新获取系统对象
                    auto new_system = _mavsdk.systems().at(0);;
                    if (new_system)
                    {
                        g_system = new_system;
                    }
                }
                if (m_stopTelemetry)
                    break;
                mp_log_info(DEFAULT_LOGGER, "UAV reconnected success");
                // 重连后，重新初始化 Telemetry、Action 和 ManualControl 插件
                m_telemetry = std::make_shared<TelemetryImpl>(g_system);
                m_wf_action = std::make_shared<ActionImpl>(g_system);
                m_wf_manual_control = std::make_shared<ManualControlImpl>(g_system);
                set_device_status(DeviceStatusTbl::DEVICE_STATE_NORMAL);
                // 重新注册所有回调
                register_telemetry_callbacks();
            }

            std::this_thread::sleep_for(std::chrono::milliseconds(500));
        }
        mp_log_info(DEFAULT_LOGGER, "Telemetry loop stopped.");
    }

    //解锁状态
    void WfManager::armd_callback(bool armd)
    {
        std::cout << "microUAV WfManager::armd_callback() Start" << std::endl;
        printf("current arm state = %s\n", armd ? "enabled" : "disabled");
        DeviceAgentModel::DeviceAgentProperty flyStatusP;
        DafModel::get_instance()->get_property("flyStatus", flyStatusP);
        (*flyStatusP.mutable_property_struct()->mutable_members())["id_armed"].mutable_property_boolean()->
                                                                               set_value(armd);
        DafModel::get_instance()->set_property("flyStatus", flyStatusP);
        std::cout << "microUAV WfManager::armd_callback() Done" << std::endl;
    }

    // 电量信息
    void WfManager::battery_status_callback(Telemetry::Battery battery)
    {
        std::cout << "microUAV WfManager::battery_status_callback() Start" << std::endl;
        printf("current battery = %f\n", battery.remaining_percent);
        DeviceAgentModel::DeviceAgentProperty batteryP;
        DafModel::get_instance()->get_property("battery", batteryP);
        batteryP.mutable_property_array()->Clear();
        DeviceAgentModel::DeviceAgentProperty batteryPropertyP;
        DafModel::get_instance()->get_property("battteryProperty", batteryPropertyP);
        (*batteryPropertyP.mutable_property_struct()->mutable_members())["id"].mutable_property_int()->
                                                                               set_value(battery.id);
        (*batteryPropertyP.mutable_property_struct()->mutable_members())["percent"].mutable_property_int()->set_value(
            int(battery.remaining_percent));
        (*batteryPropertyP.mutable_property_struct()->mutable_members())["vol"].mutable_property_int()->set_value(
            battery.voltage_v); //电压 mv
        DafModel::get_instance()->set_property("battteryProperty", batteryPropertyP);
        batteryP.mutable_property_array()->add_properties()->CopyFrom(batteryPropertyP);
        DafModel::get_instance()->set_property("battery", batteryP);
        mp_log_info(DEFAULT_LOGGER, "microUAV remaining_percent: %d", int(battery.remaining_percent));
        std::cout << "microUAV WfManager::battery_status_callback() Done" << std::endl;
    };

    // 定位信息
    void WfManager::position_callback(Telemetry::Position position)
    {
        std::cout << "microUAV WfManager::position_callback() Start" << std::endl;
        printf("current position latitude = %f,  longitude = %f, altitude = %f, relative_altitude = %f",
               position.latitude_deg,
               position.longitude_deg,
               position.absolute_altitude_m,
               position.relative_altitude_m);
        DeviceAgentModel::DeviceAgentProperty locationP;
        DafModel::get_instance()->get_property("location", locationP);
        (*locationP.mutable_property_struct()->mutable_members())["latitude"].mutable_property_double()->set_value(
            position.latitude_deg);
        (*locationP.mutable_property_struct()->mutable_members())["longitude"].mutable_property_double()->set_value(
            position.longitude_deg);
        (*locationP.mutable_property_struct()->mutable_members())["altitude"].mutable_property_float()->set_value(
            position.absolute_altitude_m); //海拔高度 int m 转 float m
        (*locationP.mutable_property_struct()->mutable_members())["relative_altitude"].mutable_property_float()->
            set_value(position.relative_altitude_m); //气压计高度 int m 转 float m
        DafModel::get_instance()->set_property("location", locationP);
        std::cout << "microUAV WfManager::position_callback() Done" << std::endl;
    };

    // 速度信息
    void WfManager::position_velocity_ned_callback(Telemetry::PositionVelocityNed velocity_ned)
    {
        std::cout << "microUAV WfManager::position_velocity_ned_callback() Start" << std::endl;
        printf("current velocity ned_north = %f, ned_east = %f, ned_v = %f", velocity_ned.velocity.north_m_s,
               velocity_ned.velocity.east_m_s, velocity_ned.velocity.down_m_s);
        DeviceAgentModel::DeviceAgentProperty velocityP;
        DafModel::get_instance()->get_property("velocity", velocityP);
        (*velocityP.mutable_property_struct()->mutable_members())["ned_north"].mutable_property_float()->set_value(
            velocity_ned.velocity.north_m_s); //偏北速度 int m/s 转 float m/s
        (*velocityP.mutable_property_struct()->mutable_members())["ned_east"].mutable_property_float()->set_value(
            velocity_ned.velocity.east_m_s); //偏东速度 int m/s 转 float m/s
        (*velocityP.mutable_property_struct()->mutable_members())["ned_v"].mutable_property_float()->
                                                                           set_value(velocity_ned.velocity.down_m_s);
        //垂直速度-向下速度 int m/s 转 float m/s
        DafModel::get_instance()->set_property("velocity", velocityP);
        std::cout << "microUAV WfManager::position_velocity_ned_callback() Done" << std::endl;
    };

    // 姿态角速度信息
    void WfManager::attitude_angular_velocity_callback(Telemetry::AngularVelocityBody angularVelocityBody)
    {
        std::cout << "microUAV WfManager::attitude_angular_velocity_callback() Start" << std::endl;
        DeviceAgentModel::DeviceAgentProperty postureP;
        DafModel::get_instance()->get_property("posture", postureP);
        (*postureP.mutable_property_struct()->mutable_members())["roll_rad_s"].mutable_property_float()->set_value(
            angularVelocityBody.roll_rad_s); //飞机横滚角 int ° 转 float °
        (*postureP.mutable_property_struct()->mutable_members())["pitch_rad_s"].mutable_property_float()->set_value(
            angularVelocityBody.pitch_rad_s); //飞机俯仰角 int ° 转 float °
        (*postureP.mutable_property_struct()->mutable_members())["yaw_rad_s"].mutable_property_float()->set_value(
            angularVelocityBody.yaw_rad_s); //航向角 int ° 转 float °
        DafModel::get_instance()->set_property("posture", postureP);
        std::cout << "microUAV WfManager::attitude_angular_velocity_callback() Done" << std::endl;
    };

    //  姿态角信息
    void WfManager::attitude_euler_callback(Telemetry::EulerAngle euler_angle)
    {
        std::cout << "microUAV WfManager::attitude_euler_callback() Start" << std::endl;
        // TODO fix
        std::cout << "microUAV WfManager::attitude_euler_callback() Done" << std::endl;
    }


    // 返航点位置信息
    void WfManager::home_position_callback(Telemetry::Position position)
    {
        std::cout << "microUAV WfManager::home_position_callback() Start" << std::endl;
        DeviceAgentModel::DeviceAgentProperty homeLocationP;
        DafModel::get_instance()->get_property("homeLocation", homeLocationP);
        (*homeLocationP.mutable_property_struct()->mutable_members())["hlat"].mutable_property_float()->
                                                                              set_value(position.latitude_deg);
        //Home点纬度 double 转 float
        (*homeLocationP.mutable_property_struct()->mutable_members())["hln"].mutable_property_float()->
                                                                             set_value(position.longitude_deg);
        //Home点经度 double 转 float
        (*homeLocationP.mutable_property_struct()->mutable_members())["halt"].mutable_property_int()->set_value(
            position.absolute_altitude_m);
        //(*homeLocationP.mutable_property_struct()->mutable_members())["dis"].mutable_property_int()->set_value(obj->dis);
        DafModel::get_instance()->set_property("homeLocation", homeLocationP);
        std::cout << "microUAV WfManager::home_position_callback() Done" << std::endl;
    };

    // 搜星数量信息
    void WfManager::gps_info_callback(Telemetry::GpsInfo gpsInfo)
    {
        std::cout << "microUAV WfManager::gps_info_callback() Start" << std::endl;
        printf("current gps search star number = %d\n", gpsInfo.num_satellites);
        DeviceAgentModel::DeviceAgentProperty flyStatusP;
        Gazella::Daf::DafModel::get_instance()->get_property("flyStatus", flyStatusP);
        // 卫星数量
        (*flyStatusP.mutable_property_struct()->mutable_members())["num_satellites"].mutable_property_int()->set_value(
            gpsInfo.num_satellites);
        Gazella::Daf::DafModel::get_instance()->set_property("flyStatus", flyStatusP);
        std::cout << "microUAV WfManager::gps_info_callback() Done" << std::endl;
    };

    // 飞行模式信息
    void WfManager::flight_mode_callback(const Telemetry::FlightMode flightMode)
    {
        //飞行模式
        std::cout << "microUAV WfManager::flight_mode_callback() Start" << std::endl;
        printf("current flight_mode = %d\n", flightMode);
        DeviceAgentModel::DeviceAgentProperty flyStatusP;
        DafModel::get_instance()->get_property("flyStatus", flyStatusP);
        (*flyStatusP.mutable_property_struct()->mutable_members())["fly_mode"].mutable_property_int()->set_value(
            static_cast<int>(fly_mode_transform(flightMode)));
        DafModel::get_instance()->set_property("flyStatus", flyStatusP);
        std::cout << "microUAV WfManager::flight_mode_callback() Done" << std::endl;
    };

    // 飞机状态更新
    void WfManager::health_status_callback(Telemetry::Health health)
    {
        std::cout << "microUAV WfManager::health_status_callback() Start" << std::endl;

        std::cout << "microUAV WfManager::health_status_callback() Done" << std::endl;
    };

    // 飞机是否处于可飞行状态
    void WfManager::health_all_callback(bool is_all_ok)
    {
        std::cout << "microUAV WfManager::health_all_callback() Start" << std::endl;
        printf("current health status: %d\n", is_all_ok);
        DeviceAgentModel::DeviceAgentProperty flyStatusP;
        DafModel::get_instance()->get_property("flyStatus", flyStatusP);
        (*flyStatusP.mutable_property_struct()->mutable_members())["is_all_ok"].mutable_property_boolean()->
            set_value(is_all_ok);
        DafModel::get_instance()->set_property("flyStatus", flyStatusP);
        getInstance()->set_device_status(DeviceStatusTbl::DEVICE_STATE_NORMAL);
        std::cout << "microUAV WfManager::health_all_callback() Done" << std::endl;
    };

    float WfManager::scaleJoyValue(int inputValue, const int outputMin, const int outputMax)
    {
        constexpr int inputMin = -1024;
        constexpr int inputMax = 1024;
        if (inputValue > 1024)
        {
            inputValue = 1024;
        }
        else if (inputValue < -1024)
        {
            inputValue = -1024;
        }

        const double scaleFactor = static_cast<double>(outputMax - outputMin) / (inputMax - inputMin);
        const auto scaledValue = static_cast<float>((inputValue - inputMin) * scaleFactor + outputMin);
        return scaledValue;
    }

    Mission::MissionItem WfManager::make_mission_item(
        const double latitude_deg,
        const double longitude_deg,
        const float relative_altitude_m,
        const float speed_m_s,
        const bool is_fly_through,
        const float loiter_time_s)
    {
        Mission::MissionItem new_item{};
        new_item.latitude_deg = latitude_deg;
        new_item.longitude_deg = longitude_deg;
        new_item.relative_altitude_m = relative_altitude_m;
        new_item.speed_m_s = speed_m_s;
        new_item.is_fly_through = is_fly_through;
        new_item.loiter_time_s = loiter_time_s;
        return new_item;
    }

    CommonFlightMode WfManager::fly_mode_transform(const Telemetry::FlightMode& flightMode)
    {
        switch (flightMode)
        {
        case Telemetry::FlightMode::Unknown:
            return CommonFlightMode::Unknown;
        case Telemetry::FlightMode::Ready:
            return CommonFlightMode::TakeoffPrepare;
        case Telemetry::FlightMode::Takeoff:
            return CommonFlightMode::AutoTakeoff;
        case Telemetry::FlightMode::Hold:
            return CommonFlightMode::Hold;
        case Telemetry::FlightMode::Mission:
            return CommonFlightMode::Mission;
        case Telemetry::FlightMode::ReturnToLaunch:
            return CommonFlightMode::ReturnToLaunch;
        case Telemetry::FlightMode::Land:
            return CommonFlightMode::AutoLand;
        case Telemetry::FlightMode::Offboard:
            return CommonFlightMode::Offboard;
        case Telemetry::FlightMode::FollowMe:
            return CommonFlightMode::FollowMe;
        case Telemetry::FlightMode::Manual:
            return CommonFlightMode::Manual;
        case Telemetry::FlightMode::Altctl:
            return CommonFlightMode::Altctl;
        case Telemetry::FlightMode::Posctl:
            return CommonFlightMode::Posctl;
        case Telemetry::FlightMode::Acro:
            return CommonFlightMode::Acro;
        case Telemetry::FlightMode::Stabilized:
            return CommonFlightMode::Stabilized;
        case Telemetry::FlightMode::Rattitude:
            return CommonFlightMode::Rattitude;
        default:
            return CommonFlightMode::Unknown;
        }
    }
}
